/*
 * solver.c
 *
 *  Created on: 20.11.2012
 *      Author: Myrex
 */

#include <string.h>
#include <math.h>

#include <navi.h>
#include <matrix.h>
#include <quater.h>

#include <debug.h>

static quater_t bkv, isk;
static navi_state_e sts;
static double lat, lon;
static char got_lat, got_lon;
//static double EarthRot = 0.000072722;
//static double g = 9.81;
static double vx, vy, vz;
static double wx, wy, wz;
static matrix_t gyr, gyrs ,aks, mnk, i_aks, tmp, diff;
static double _x, _y, _z;

int input_start();
mtx_err_e get_accel_data(matrix_t *dat);
mtx_err_e get_gyro_data(matrix_t *dat);

double atan_ex(double s, double c) {
	double ret = 0;
	if (s == 0 && c == 0) {
		ret = 0;
	} else if (c == 0) {
		if (s > 0) {
			ret = PI * 0.5;
		} else {
			ret = PI * 1.5;
		}
	} else {
		ret = atan(s / c);
		if (c < 0) {
			ret += PI;
		} else if (s < 0) {
			ret += 2. * PI;
		}
	}

	return ret;
}

//static mtx_err_e build_mnk(matrix_t *m, quater_t *q) {
//	mtx_err_e ret = mtx_ok;
//
//	m->data[0][0] = q->re * q->re + q->i * q->i - q->j * q->j - q->k * q->k;
//	m->data[0][1] = 2 * q->i * q->j + 2 * q->re * q->k;
//	m->data[0][2] = 2 * q->i * q->k - 2 * q->re * q->j;
//
//	m->data[1][0] = 2 * q->i * q->j - 2 * q->re * q->k;
//	m->data[1][1] = 2 * q->re * q->re + 2 * q->j * q->j - q->i * q->i
//			- q->k * q->k;
//	m->data[1][2] = 2 * q->j * q->k + 2 * q->re * q->i;
//
//	m->data[2][0] = 2 * q->i * q->k + 2 * q->re * q->j;
//	m->data[2][1] = 2 * q->j * q->k - 2 * q->re * q->i;
//	m->data[2][2] = 2 * q->re * q->re + 2 * q->k * q->i - q->i * q->i
//			- q->j * q->j;
//
//	return ret;
//
//}



static mtx_err_e build_rot_m(matrix_t *m, matrix_t *g) {
	mtx_err_e ret = mtx_ok;

	m->data[0][0] = 0;
	m->data[0][1] = g->data[2][0];//wz
	m->data[0][2] = -g->data[1][0];//-wy

	m->data[1][0] =  -g->data[2][0];//-wz
	m->data[1][1] = 0;
	m->data[1][2] = g->data[0][0];//wx

	m->data[2][0] = g->data[1][0];//wy
	m->data[2][1] = -g->data[0][0];//-wx
	m->data[2][2] = 0;

	return ret;

}


//static double Ra(double ln) {
//	return 6400000.;
//}
//
//static double Rb(double la) {
//	return 6400000.;
//}

int navi_start() {

	int ret = 0;

	memset(&bkv, 0, sizeof(quater_t));
	memset(&isk, 0, sizeof(quater_t));

	memset(&mnk, 0, sizeof(quater_t));

	sts = navi_rough;
	got_lat = 0;
	got_lon = 0;
	ret = input_start();

	ret = mtx_init(&gyr, 1, 3);
	ret = mtx_init(&gyrs, 3, 3);
	ret = mtx_init(&aks, 1, 3);
	ret = mtx_init(&mnk, 3, 3);
	ret = mtx_init(&tmp, 3, 3);
	ret = mtx_init(&diff, 3, 3);
	ret = mtx_init(&i_aks, 1, 3);

	_x = _y = _z = 0;

	vx = vy = vz = 0;
	wx = wy = wz = 0;


	 mtx_set_E(&mnk);


	return ret;
}

int navi_step(navi_t *inf) {
	int ret = 0;

	memset(inf, 0, sizeof(navi_t));

	double h = STEP;
	//standart routine
	get_accel_data(&aks);
	get_gyro_data(&gyr);

	/******************************************/
	/*             SOLVING START              */
	/******************************************/
	build_rot_m(&gyrs,&gyr);

//slove step

	 mtx_copy(&tmp, &mnk);

	 ret = mtx_mul(&diff, &gyrs, &tmp);
	 //integrating
	 mtx_scm(&tmp, &diff, h);

	 mtx_copy(&diff, &tmp);

	 mtx_sum(&tmp,&mnk,&diff);

	 mtx_copy(&mnk, &tmp);

	 //angles out
	 /*
	 	 inf->isk.theta = asin(mnk.data[1][2]);

	 	 inf->isk.psi = atan_ex(  -(mnk.data[1][0]), mnk.data[1][1] );

	 	 inf->isk.gamma = atan_ex(  -(mnk.data[0][2]), mnk.data[2][2] );
	 	 //*/

	 /*
	 	double s = mnk.data[1][2]*mnk.data[1][2];
	 	s = sqrt (1.0 - s);

	 	 inf->isk.theta = asin(mnk.data[1][2]);

	 	 inf->isk.psi = atan_ex( -1.*(mnk.data[1][0])/s, (mnk.data[1][1])/s);

	 	 inf->isk.gamma = atan_ex(-1. * (mnk.data[0][2])/s, (mnk.data[2][2])/s );
	 	//*/


	 	double s = mnk.data[0][1]*mnk.data[0][1];
	 	s = 1.0;//sqrt (1.0 - s);

	 	 inf->isk.theta = asin(mnk.data[0][1]);

	 	 inf->isk.psi = atan_ex((mnk.data[0][2])/s, (mnk.data[0][0])/s);

	 	 inf->isk.gamma = atan_ex(-1. * (mnk.data[2][1])/s, (mnk.data[1][1])/s );
	 	 //*/

	 /*

	 	inf->isk.theta = asin(mnk.data[0][1]);
	 	inf->isk.psi = atan((mnk.data[0][2]) /(mnk.data[0][0]) );
	 	inf->isk.gamma = atan(-1. * (mnk.data[2][1]) / (mnk.data[1][1]));
	 	//*/

	 //basic data

	 		inf->coord.lat = lat;
	 		inf->coord.lon = lon;


	return ret;

}

void navi_set_lat(double nlat) {
	lat = nlat;
	got_lat = 1;
}

void navi_set_lon(double nlon) {
	lon = nlon;
	got_lon = 1;
}

